#include <caros/kuka_lwr.h>
#include <std_msgs/Float64.h>
#include <string>
#include <vector>

kuka_lwr::kuka_lwr(std::string topic)
    : _current_force_q(7),
      _current_q(7),
      _n("~"),
      _cart_force_pub(_n.advertise<geometry_msgs::WrenchStamped>(topic, 1, true)),
      _device_state(_n.advertise<caros_control_msgs::RobotState>(
          "/caros_kuka_lwr/caros_serial_device_service_interface/robot_state", 1, true)),
      _cart_pos_pub(_n.advertise<std_msgs::Float64>("/caros_kuka_lwr/z_pos", 1, true)),
      new_message_(false),
      success_(true)
{
  std::vector<std::string> reset_val(7, "0");
  set_cart_force(reset_val, 0);
  set_force_q(reset_val, 0);
  set_q(reset_val, 0);
  set_pos(reset_val, 0);
  set_moving(false);
  _current_cart_force.header.frame_id = "FT";
}

void kuka_lwr::publish()
{
  _cart_force_pub.publish(_current_cart_force);
  _device_state.publish(to_caros());
  std_msgs::Float64 msg;
  msg.data = _current_pos.P()(2);
  _cart_pos_pub.publish(msg);
}

caros_control_msgs::RobotState kuka_lwr::to_caros()
{
  caros_control_msgs::RobotState robot_state;

  robot_state.header.stamp = ros::Time::now();
  robot_state.header.frame_id = _n.getNamespace();

  robot_state.q = caros::toRos(_current_q);
  // std::cout << "q: " << _current_q << std::endl;
  // TODO(nivii): calculate velocity in radians/sec
  // robot_state.dq              = caros::toRos(dq);
  robot_state.tcp_force = _current_cart_force.wrench;
  robot_state.is_moving = _is_moving;

  // TODO(nivii): The calls over tcp can return false when something fails, but no method can currently see the reason.
  // robot_state.is_colliding = false;
  robot_state.e_stopped = false;
  robot_state.s_stopped = false;

  return robot_state;
}

void kuka_lwr::set_cart_force(std::vector<std::string> &variables, size_t start_index)
{
  if ((start_index + 6) < variables.size())
  {
    _current_cart_force.wrench.force.x = atof(variables.at(start_index + 1).c_str());
    _current_cart_force.wrench.force.y = atof(variables.at(start_index + 2).c_str());
    _current_cart_force.wrench.force.z = atof(variables.at(start_index + 3).c_str());
    _current_cart_force.wrench.torque.x = atof(variables.at(start_index + 4).c_str());
    _current_cart_force.wrench.torque.y = atof(variables.at(start_index + 5).c_str());
    _current_cart_force.wrench.torque.z = atof(variables.at(start_index + 6).c_str());
    _current_cart_force.header.stamp = ros::Time::now();
  }
}

void kuka_lwr::set_force_q(std::vector<std::string> &variables, size_t start_index)
{
  if ((start_index + 7) < variables.size())
  {
    _current_force_q[0] = atof(variables.at(start_index + 1).c_str());
    _current_force_q[1] = atof(variables.at(start_index + 2).c_str());
    _current_force_q[2] = atof(variables.at(start_index + 3).c_str());
    _current_force_q[3] = atof(variables.at(start_index + 4).c_str());
    _current_force_q[4] = atof(variables.at(start_index + 5).c_str());
    _current_force_q[5] = atof(variables.at(start_index + 6).c_str());
    _current_force_q[6] = atof(variables.at(start_index + 7).c_str());
  }
}

void kuka_lwr::set_q(std::vector<std::string> &variables, size_t start_index)
{
  if ((start_index + 7) < variables.size())
  {
    _current_q[0] = atof(variables.at(start_index + 1).c_str());
    _current_q[1] = atof(variables.at(start_index + 2).c_str());
    _current_q[2] = atof(variables.at(start_index + 3).c_str());
    _current_q[3] = atof(variables.at(start_index + 4).c_str());
    _current_q[4] = atof(variables.at(start_index + 5).c_str());
    _current_q[5] = atof(variables.at(start_index + 6).c_str());
    _current_q[6] = atof(variables.at(start_index + 7).c_str());
  }
}

void kuka_lwr::set_pos(std::vector<std::string> &variables, size_t start_index)
{
  if ((start_index + 6) < variables.size())
  {
    double x = atof(variables.at(start_index + 1).c_str());
    double y = atof(variables.at(start_index + 2).c_str());
    double z = atof(variables.at(start_index + 3).c_str());
    double roll = atof(variables.at(start_index + 4).c_str());
    double pitch = atof(variables.at(start_index + 5).c_str());
    double yaw = atof(variables.at(start_index + 6).c_str());
    rw::math::Vector3D<double> pos(x, y, z);
    rw::math::RPY<double> rot(roll, pitch, yaw);
    _current_pos = rw::math::Transform3D<double>(pos * 0.001, rot);  // received pos is in [mm]
  }
}
void kuka_lwr::set_moving(bool is_moving)
{
  _is_moving = is_moving;
}
